package orbito.physics.motion;

import java.util.LinkedList;
import java.util.List;

import orbito.physics.motion.State.Derivative;

public class RungeKuttaIntegrator implements Integrator {

	Actor[] actors;
	List<Actor> actorsList;
	
	ForceManager forceManager;
	
	public RungeKuttaIntegrator(ForceManager forceManager) {
		this.actorsList = new LinkedList<Actor>();
		this.forceManager = forceManager;
	}
	
	public List<Actor> getActors() {
		return actorsList;
	}
	
	public void addActor(Actor actor) {
		actorsList.add(actor);
		actors = null;
	}
	
	public void removeActor(Actor actor) {
		actorsList.remove(actor);
		actors = null;
	}
	
	public ForceManager getForceManager() {
		return forceManager;
	}
	
	public void setForceManager(ForceManager forceManager) {
		this.forceManager = forceManager;
	}
	
	public void integrate(double t, double dt) {
		// Lazy creation of actors array.
		if (actors == null) {
			actors = new Actor[actorsList.size()];
			actorsList.toArray(actors);
		}
		
		// The derivative samples.
		Derivative[] a = new Derivative[actors.length];
		Derivative[] b = new Derivative[actors.length];
		Derivative[] c = new Derivative[actors.length];
		Derivative[] d = new Derivative[actors.length];
		
		// The initial states at time t.
		State[] initialStates = new State[actors.length];
		
		// Calculate forces at time t.
		forceManager.calculateForces(t);
		
		// Save states at time t.
		for (int i = 0; i < actors.length; i++) {
			initialStates[i] = actors[i].state;
		}
		
		// Evaluate first derivatives at time t, and calculate states at time t+.5dt, using first derivatives.
		for (int i = 0; i < actors.length; i++) {
			a[i] = evaluate(actors[i]);
			actors[i].state = calculate(initialStates[i], dt * 0.5, a[i]);
		}
		
		// Recalculate forces for new states at time t+.5dt.
		forceManager.calculateForces(t + dt * 0.5);
		
		// Evaluate second derivatives at time t+.5dt, and calculate states at time t+.5dt (again), using second derivatives.
		for (int i = 0; i < actors.length; i++) {
			b[i] = evaluate(actors[i]);
			actors[i].state = calculate(initialStates[i], dt * 0.5, b[i]);
		}
		
		// Recalculate forces for new states at time t+.5dt.
		forceManager.calculateForces(t + dt * 0.5);
		
		// Evaluate third derivatives at time t+.5dt, and calculate states at time t+dt, using third derivatives.
		for (int i = 0; i < actors.length; i++) {
			c[i] = evaluate(actors[i]);
			actors[i].state = calculate(initialStates[i], dt, c[i]);
		}
		
		// Recalculate forces for new states at time t+dt.
		forceManager.calculateForces(t + dt);
		
		// Two things can now be done in the same iteration.
		for (int i = 0; i < actors.length; i++) {
			// 1. Evaluate fourth derivatives at time t+dt.
			d[i] = evaluate(actors[i]);
			
			// 2. Calculate weighted average of derivatives, and calculate the final state at time t+dt using the averaged derivative.
			// Note that by passing simply the a-derivative in the stead of all four derivatives, this becomes an euler-integrator.
			Derivative avg = average(initialStates[i], a[i], b[i], c[i], d[i]);
			actors[i].state = calculate(initialStates[i], dt, avg);
		} 
	}
	
	private Derivative evaluate(Actor actor) {
		return new Derivative(
				actor.state.v,
				actor.getAcceleration(),
				actor.state.a,
				actor.getAngularAcceleration());
	}
	
	private State calculate(State initial, double dt, Derivative d) {
		return new State(
				initial.x.add(d.dx.mul(dt)),
				initial.v.add(d.dv.mul(dt)),
				initial.a + d.da * dt,
				initial.w + d.dw * dt);
	}
	
	private Derivative average(State initial, Derivative a, Derivative b, Derivative c, Derivative d) {
		// The weighted average of the 4 derivatives is computed as follows:
		// dx/dt = (a + 2b + 2c + d) / 6 = (a + 2(a + b) + d)(1 / 6)
		double factor = 1.0 / 6.0;
		return new Derivative(
				a.dx.add(b.dx.add(c.dx).mul(2)).add(d.dx).mul(factor),
				a.dv.add(b.dv.add(c.dv).mul(2)).add(d.dv).mul(factor),
				(a.da + (b.da + c.da) * 2 + d.da) * factor,
				(a.dw + (b.dw + c.dw) * 2 + d.dw) * factor);
	}
}
